#include "CaptureFlagState.h"
#include "AttractiveField.h"
#include "World.h"
#include "PDController.h"
#include "FollowSearchPathState.h"
#include "Graph.h"

bool CaptureFlagState::flag_captured = false;

CaptureFlagState::CaptureFlagState(void)
{
}


CaptureFlagState::~CaptureFlagState(void)
{
}

Command CaptureFlagState::GetCommand(MultiAgent* agent) {
	World* world = World::instance();
	tank_t* tank = agent->getTank();
	flag_t flag = world->flags().at(2);

	if (tank->flag == flag.color) {
		flag_captured = true;

		Vector2 start, goal;

		start.x =  tank->pos[0];
		start.y =  tank->pos[1];

		goal.x = world->flags()[3].pos[0];
		goal.y = world->flags()[3].pos[1];
		Graph g(8);
		g.buildEdges();

		agent->setState(new FollowSearchPathState(&g, start, goal, false));
		return agent->GetCommand();
	}

	AttractiveField target(Vector2(flag.pos[0], flag.pos[1]));
	Vector2 trajectory = target.get_trajectory(Vector2(tank->pos[0], tank->pos[1]));
	
	return PDController::instance()->GetComand(tank, trajectory);
}

bool CaptureFlagState::IsFlagCaptured() {
	return flag_captured;
}